function dxdt = dstate(t,x,u)
% returns the state variables derivative 
% Last modification: 30/03/2014 (by Feliphe G. Galiza)

% state variables
theta1 = x(1);
theta2 = x(2);
theta3= x(3);
theta4 = x(4);
theta5 = x(5);
theta1_dot = x(6);
theta2_dot = x(7);
theta3_dot = x(8);
theta4_dot  = x(9);
theta5_dot = x(10);

% parameters according to human body
height = 1.5; % [m]
L1 = 0.285*height; % [m]
L2 = (0.53 - 0.285)*height; % [m]
L3 = height - (L1 + L2); % [m]
L4 = L2; % [m]
L5 =L1; % [m]

r1 = L1/2; % [m]
r2 = L2/2; % [m]
r3 = L3/2; % [m]    
r4 = L4/2; % [m]
r5 = L5/2; % [m]

mass = 50; % [kg]
m1 = 0.061 * mass; % [kg]
m2 = 0.1 * mass; % [kg]
m3 = 0.678 * mass; % [kg]
m4 = m2; % [kg]
m5 = m1; % [kg]

Izz_1 = m1 * 0.735^2; % [kg*m^2]
Izz_2 = m2 * 0.540^2; % [kg*m^2]
Izz_3 = m3 * 0.0798^2; % [kg*m^2]
Izz_4 = Izz_2; % [kg*m^2]
Izz_5 = Izz_1; % [kg*m^2]

% Equation of Motion
M(1,:) = [ Izz_1 + Izz_2 + Izz_3 + Izz_4 + Izz_5 + m4*(L2*cos(theta1 + theta2) + L1*cos(theta1) - r4*cos(theta1 + theta2 + theta3 - theta4))^2 + m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4))^2 + m3*(L2*cos(theta1 + theta2) + L1*cos(theta1) + r3*cos(theta1 + theta2 + theta3))^2 + m4*(L2*sin(theta1 + theta2) + L1*sin(theta1) - r4*sin(theta1 + theta2 + theta3 - theta4))^2 + m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4))^2 + m3*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2) + L1*sin(theta1))^2 + m2*(L1*cos(theta1) + cos(theta1 + theta2)*(L2 - r2))^2 + m2*(L1*sin(theta1) + sin(theta1 + theta2)*(L2 - r2))^2 + m1*cos(theta1)^2*(L1 - r1)^2 + m1*sin(theta1)^2*(L1 - r1)^2, Izz_2 + Izz_3 + Izz_4 + Izz_5 + m4*(L2*cos(theta1 + theta2) - r4*cos(theta1 + theta2 + theta3 - theta4))*(L2*cos(theta1 + theta2) + L1*cos(theta1) - r4*cos(theta1 + theta2 + theta3 - theta4)) + m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)) + m4*(L2*sin(theta1 + theta2) - r4*sin(theta1 + theta2 + theta3 - theta4))*(L2*sin(theta1 + theta2) + L1*sin(theta1) - r4*sin(theta1 + theta2 + theta3 - theta4)) + m3*(L2*cos(theta1 + theta2) + r3*cos(theta1 + theta2 + theta3))*(L2*cos(theta1 + theta2) + L1*cos(theta1) + r3*cos(theta1 + theta2 + theta3)) + m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)) + m3*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2))*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2) + L1*sin(theta1)) + m2*cos(theta1 + theta2)*(L1*cos(theta1) + cos(theta1 + theta2)*(L2 - r2))*(L2 - r2) + m2*sin(theta1 + theta2)*(L2 - r2)*(L1*sin(theta1) + sin(theta1 + theta2)*(L2 - r2)), Izz_3 + Izz_4 + Izz_5 + m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)) + m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)) - m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) + L1*cos(theta1) - r4*cos(theta1 + theta2 + theta3 - theta4)) - m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) + L1*sin(theta1) - r4*sin(theta1 + theta2 + theta3 - theta4)) + m3*r3*cos(theta1 + theta2 + theta3)*(L2*cos(theta1 + theta2) + L1*cos(theta1) + r3*cos(theta1 + theta2 + theta3)) + m3*r3*sin(theta1 + theta2 + theta3)*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2) + L1*sin(theta1)), m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) + L1*cos(theta1) - r4*cos(theta1 + theta2 + theta3 - theta4)) - Izz_5 - m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)) - Izz_4 + m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) + L1*sin(theta1) - r4*sin(theta1 + theta2 + theta3 - theta4)), Izz_5 + m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)) + m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4))];
M(2,:) = [ Izz_2 + Izz_3 + Izz_4 + Izz_5 + m4*(L2*cos(theta1 + theta2) - r4*cos(theta1 + theta2 + theta3 - theta4))*(L2*cos(theta1 + theta2) + L1*cos(theta1) - r4*cos(theta1 + theta2 + theta3 - theta4)) + m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)) + m4*(L2*sin(theta1 + theta2) - r4*sin(theta1 + theta2 + theta3 - theta4))*(L2*sin(theta1 + theta2) + L1*sin(theta1) - r4*sin(theta1 + theta2 + theta3 - theta4)) + m3*(L2*cos(theta1 + theta2) + r3*cos(theta1 + theta2 + theta3))*(L2*cos(theta1 + theta2) + L1*cos(theta1) + r3*cos(theta1 + theta2 + theta3)) + m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)) + m3*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2))*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2) + L1*sin(theta1)) + m2*cos(theta1 + theta2)*(L1*cos(theta1) + cos(theta1 + theta2)*(L2 - r2))*(L2 - r2) + m2*sin(theta1 + theta2)*(L2 - r2)*(L1*sin(theta1) + sin(theta1 + theta2)*(L2 - r2)), Izz_2 + Izz_3 + Izz_4 + Izz_5 + m4*(L2*cos(theta1 + theta2) - r4*cos(theta1 + theta2 + theta3 - theta4))^2 + m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4))^2 + m4*(L2*sin(theta1 + theta2) - r4*sin(theta1 + theta2 + theta3 - theta4))^2 + m3*(L2*cos(theta1 + theta2) + r3*cos(theta1 + theta2 + theta3))^2 + m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4))^2 + m3*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2))^2 + m2*cos(theta1 + theta2)^2*(L2 - r2)^2 + m2*sin(theta1 + theta2)^2*(L2 - r2)^2, Izz_3 + Izz_4 + Izz_5 + m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)) + m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)) + m3*r3*cos(theta1 + theta2 + theta3)*(L2*cos(theta1 + theta2) + r3*cos(theta1 + theta2 + theta3)) + m3*r3*sin(theta1 + theta2 + theta3)*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2)) - m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) - r4*cos(theta1 + theta2 + theta3 - theta4)) - m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) - r4*sin(theta1 + theta2 + theta3 - theta4)), m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) - r4*cos(theta1 + theta2 + theta3 - theta4)) - Izz_5 - m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)) - Izz_4 + m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) - r4*sin(theta1 + theta2 + theta3 - theta4)), Izz_5 + m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)) + m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4))];
M(3,:) = [ Izz_3 + Izz_4 + Izz_5 + m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)) + m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)) - m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) + L1*cos(theta1) - r4*cos(theta1 + theta2 + theta3 - theta4)) - m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) + L1*sin(theta1) - r4*sin(theta1 + theta2 + theta3 - theta4)) + m3*r3*cos(theta1 + theta2 + theta3)*(L2*cos(theta1 + theta2) + L1*cos(theta1) + r3*cos(theta1 + theta2 + theta3)) + m3*r3*sin(theta1 + theta2 + theta3)*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2) + L1*sin(theta1)), Izz_3 + Izz_4 + Izz_5 + m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)) + m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)) + m3*r3*cos(theta1 + theta2 + theta3)*(L2*cos(theta1 + theta2) + r3*cos(theta1 + theta2 + theta3)) + m3*r3*sin(theta1 + theta2 + theta3)*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2)) - m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) - r4*cos(theta1 + theta2 + theta3 - theta4)) - m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) - r4*sin(theta1 + theta2 + theta3 - theta4)), Izz_3 + Izz_4 + Izz_5 + m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))^2 + m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))^2 + m4*r4^2*sin(theta1 + theta2 + theta3 - theta4)^2 + m3*r3^2*cos(theta1 + theta2 + theta3)^2 + m3*r3^2*sin(theta1 + theta2 + theta3)^2 + m4*r4^2*cos(theta1 + theta2 + theta3 - theta4)^2, - Izz_4 - Izz_5 - m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))^2 - m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))^2 - m4*r4^2*sin(theta1 + theta2 + theta3 - theta4)^2 - m4*r4^2*cos(theta1 + theta2 + theta3 - theta4)^2, Izz_5 + m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4)) + m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))];
M(4,:) = [ m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) + L1*cos(theta1) - r4*cos(theta1 + theta2 + theta3 - theta4)) - Izz_5 - m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)) - Izz_4 + m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) + L1*sin(theta1) - r4*sin(theta1 + theta2 + theta3 - theta4)), m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) - r4*cos(theta1 + theta2 + theta3 - theta4)) - Izz_5 - m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)) - Izz_4 + m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) - r4*sin(theta1 + theta2 + theta3 - theta4)), - Izz_4 - Izz_5 - m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))^2 - m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))^2 - m4*r4^2*sin(theta1 + theta2 + theta3 - theta4)^2 - m4*r4^2*cos(theta1 + theta2 + theta3 - theta4)^2, Izz_4 + Izz_5 + m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))^2 + m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))^2 + m4*r4^2*sin(theta1 + theta2 + theta3 - theta4)^2 + m4*r4^2*cos(theta1 + theta2 + theta3 - theta4)^2, - Izz_5 - m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4)) - m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))];
M(5,:) = [ Izz_5 + m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)) + m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)), Izz_5 + m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)) + m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)), Izz_5 + m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4)) + m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)), - Izz_5 - m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4)) - m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)), Izz_5 + m5*r5^2*cos(theta1 + theta2 + theta3 - theta4 + theta5)^2 + m5*r5^2*sin(theta1 + theta2 + theta3 - theta4 + theta5)^2];

RHS(1,1) = u(1) + theta3_dot^2*(m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)) - m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) + L1*cos(theta1) - r4*cos(theta1 + theta2 + theta3 - theta4)) + m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) + L1*sin(theta1) - r4*sin(theta1 + theta2 + theta3 - theta4)) + m3*r3*sin(theta1 + theta2 + theta3)*(L2*cos(theta1 + theta2) + L1*cos(theta1) + r3*cos(theta1 + theta2 + theta3)) - m3*r3*cos(theta1 + theta2 + theta3)*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2) + L1*sin(theta1))) - (981*m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)))/100 + (981*m3*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2) + L1*sin(theta1)))/100 + theta2_dot^2*(m4*(L2*sin(theta1 + theta2) - r4*sin(theta1 + theta2 + theta3 - theta4))*(L2*cos(theta1 + theta2) + L1*cos(theta1) - r4*cos(theta1 + theta2 + theta3 - theta4)) - m4*(L2*cos(theta1 + theta2) - r4*cos(theta1 + theta2 + theta3 - theta4))*(L2*sin(theta1 + theta2) + L1*sin(theta1) - r4*sin(theta1 + theta2 + theta3 - theta4)) + m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)) + m3*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2))*(L2*cos(theta1 + theta2) + L1*cos(theta1) + r3*cos(theta1 + theta2 + theta3)) - m3*(L2*cos(theta1 + theta2) + r3*cos(theta1 + theta2 + theta3))*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2) + L1*sin(theta1)) + m2*sin(theta1 + theta2)*(L1*cos(theta1) + cos(theta1 + theta2)*(L2 - r2))*(L2 - r2) - m2*cos(theta1 + theta2)*(L2 - r2)*(L1*sin(theta1) + sin(theta1 + theta2)*(L2 - r2))) + theta5_dot^2*(m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4))) + (981*m2*(L1*sin(theta1) + sin(theta1 + theta2)*(L2 - r2)))/100 + theta4_dot^2*(m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)) - m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) + L1*cos(theta1) - r4*cos(theta1 + theta2 + theta3 - theta4)) + m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) + L1*sin(theta1) - r4*sin(theta1 + theta2 + theta3 - theta4))) + (981*m4*(L2*sin(theta1 + theta2) + L1*sin(theta1) - r4*sin(theta1 + theta2 + theta3 - theta4)))/100 + theta2_dot*theta5_dot*(m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)) + m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4))) - theta2_dot*theta5_dot*(m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)) - m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)) + m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4))) + theta1_dot*theta3_dot*(2*m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)) - 2*m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)) - 2*m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) + L1*cos(theta1) - r4*cos(theta1 + theta2 + theta3 - theta4)) + 2*m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) + L1*sin(theta1) - r4*sin(theta1 + theta2 + theta3 - theta4)) + 2*m3*r3*sin(theta1 + theta2 + theta3)*(L2*cos(theta1 + theta2) + L1*cos(theta1) + r3*cos(theta1 + theta2 + theta3)) - 2*m3*r3*cos(theta1 + theta2 + theta3)*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2) + L1*sin(theta1))) + (981*m1*sin(theta1)*(L1 - r1))/100 + theta1_dot*theta2_dot*(2*m4*(L2*sin(theta1 + theta2) - r4*sin(theta1 + theta2 + theta3 - theta4))*(L2*cos(theta1 + theta2) + L1*cos(theta1) - r4*cos(theta1 + theta2 + theta3 - theta4)) - 2*m4*(L2*cos(theta1 + theta2) - r4*cos(theta1 + theta2 + theta3 - theta4))*(L2*sin(theta1 + theta2) + L1*sin(theta1) - r4*sin(theta1 + theta2 + theta3 - theta4)) + 2*m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)) - 2*m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)) + 2*m3*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2))*(L2*cos(theta1 + theta2) + L1*cos(theta1) + r3*cos(theta1 + theta2 + theta3)) - 2*m3*(L2*cos(theta1 + theta2) + r3*cos(theta1 + theta2 + theta3))*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2) + L1*sin(theta1)) + 2*m2*sin(theta1 + theta2)*(L1*cos(theta1) + cos(theta1 + theta2)*(L2 - r2))*(L2 - r2) - 2*m2*cos(theta1 + theta2)*(L2 - r2)*(L1*sin(theta1) + sin(theta1 + theta2)*(L2 - r2))) + theta3_dot*theta5_dot*(m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)) + m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)) - m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))) + theta3_dot*theta5_dot*(m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)) + m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))) - theta4_dot*theta5_dot*(m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)) + m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)) - m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))) - theta4_dot*theta5_dot*(m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)) + m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))) + theta1_dot*theta5_dot*(2*m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)) - 2*m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4))) + theta2_dot*theta3_dot*(m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)) + m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)) + m3*r3*sin(theta1 + theta2 + theta3)*(L2*cos(theta1 + theta2) + r3*cos(theta1 + theta2 + theta3)) - m3*r3*cos(theta1 + theta2 + theta3)*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2)) - m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) + L1*cos(theta1) - r4*cos(theta1 + theta2 + theta3 - theta4)) + m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) + L1*sin(theta1) - r4*sin(theta1 + theta2 + theta3 - theta4)) + m3*r3*sin(theta1 + theta2 + theta3)*(L2*cos(theta1 + theta2) + L1*cos(theta1) + r3*cos(theta1 + theta2 + theta3)) - m3*r3*cos(theta1 + theta2 + theta3)*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2) + L1*sin(theta1)) - m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) - r4*cos(theta1 + theta2 + theta3 - theta4)) + m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) - r4*sin(theta1 + theta2 + theta3 - theta4))) + theta2_dot*theta3_dot*(m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)) - m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)) + m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)) - m3*r3*sin(theta1 + theta2 + theta3)*(L2*cos(theta1 + theta2) + r3*cos(theta1 + theta2 + theta3)) + m3*r3*cos(theta1 + theta2 + theta3)*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2)) - m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) + L1*cos(theta1) - r4*cos(theta1 + theta2 + theta3 - theta4)) + m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) + L1*sin(theta1) - r4*sin(theta1 + theta2 + theta3 - theta4)) + m3*r3*sin(theta1 + theta2 + theta3)*(L2*cos(theta1 + theta2) + L1*cos(theta1) + r3*cos(theta1 + theta2 + theta3)) - m3*r3*cos(theta1 + theta2 + theta3)*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2) + L1*sin(theta1)) + m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) - r4*cos(theta1 + theta2 + theta3 - theta4)) - m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) - r4*sin(theta1 + theta2 + theta3 - theta4))) - 2*theta3_dot*theta4_dot*(m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)) - m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) + L1*cos(theta1) - r4*cos(theta1 + theta2 + theta3 - theta4)) + m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) + L1*sin(theta1) - r4*sin(theta1 + theta2 + theta3 - theta4))) - theta1_dot*theta4_dot*(2*m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)) - 2*m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)) - 2*m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) + L1*cos(theta1) - r4*cos(theta1 + theta2 + theta3 - theta4)) + 2*m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) + L1*sin(theta1) - r4*sin(theta1 + theta2 + theta3 - theta4))) - theta2_dot*theta4_dot*(m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)) + m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)) - m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) + L1*cos(theta1) - r4*cos(theta1 + theta2 + theta3 - theta4)) + m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) + L1*sin(theta1) - r4*sin(theta1 + theta2 + theta3 - theta4)) - m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) - r4*cos(theta1 + theta2 + theta3 - theta4)) + m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) - r4*sin(theta1 + theta2 + theta3 - theta4))) - theta2_dot*theta4_dot*(m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)) - m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)) + m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)) - m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) + L1*cos(theta1) - r4*cos(theta1 + theta2 + theta3 - theta4)) + m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) + L1*sin(theta1) - r4*sin(theta1 + theta2 + theta3 - theta4)) + m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) - r4*cos(theta1 + theta2 + theta3 - theta4)) - m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) - r4*sin(theta1 + theta2 + theta3 - theta4)));
RHS(2,1) = u(2) - theta1_dot^2*(m4*(L2*sin(theta1 + theta2) - r4*sin(theta1 + theta2 + theta3 - theta4))*(L2*cos(theta1 + theta2) + L1*cos(theta1) - r4*cos(theta1 + theta2 + theta3 - theta4)) - m4*(L2*cos(theta1 + theta2) - r4*cos(theta1 + theta2 + theta3 - theta4))*(L2*sin(theta1 + theta2) + L1*sin(theta1) - r4*sin(theta1 + theta2 + theta3 - theta4)) + m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)) + m3*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2))*(L2*cos(theta1 + theta2) + L1*cos(theta1) + r3*cos(theta1 + theta2 + theta3)) - m3*(L2*cos(theta1 + theta2) + r3*cos(theta1 + theta2 + theta3))*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2) + L1*sin(theta1)) + m2*sin(theta1 + theta2)*(L1*cos(theta1) + cos(theta1 + theta2)*(L2 - r2))*(L2 - r2) - m2*cos(theta1 + theta2)*(L2 - r2)*(L1*sin(theta1) + sin(theta1 + theta2)*(L2 - r2))) + theta5_dot^2*(m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4))) + (981*m4*(L2*sin(theta1 + theta2) - r4*sin(theta1 + theta2 + theta3 - theta4)))/100 - (981*m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)))/100 + (981*m3*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2)))/100 + theta4_dot^2*(m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)) - m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) - r4*cos(theta1 + theta2 + theta3 - theta4)) + m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) - r4*sin(theta1 + theta2 + theta3 - theta4))) + theta3_dot^2*(m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)) + m3*r3*sin(theta1 + theta2 + theta3)*(L2*cos(theta1 + theta2) + r3*cos(theta1 + theta2 + theta3)) - m3*r3*cos(theta1 + theta2 + theta3)*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2)) - m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) - r4*cos(theta1 + theta2 + theta3 - theta4)) + m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) - r4*sin(theta1 + theta2 + theta3 - theta4))) + (981*m2*sin(theta1 + theta2)*(L2 - r2))/100 + theta1_dot*theta5_dot*((m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 - (m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 + (m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2) + theta1_dot*theta5_dot*((3*m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (3*m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 + (m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2) + theta3_dot*theta5_dot*(m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)) + m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))) + theta3_dot*theta5_dot*(m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)) - m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)) + m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))) - theta4_dot*theta5_dot*(m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)) + m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))) - theta4_dot*theta5_dot*(m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)) - m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)) + m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))) + theta2_dot*theta5_dot*(2*m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)) - 2*m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4))) - theta1_dot*theta3_dot*((m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 - (m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 + (m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 - (m3*r3*sin(theta1 + theta2 + theta3)*(L2*cos(theta1 + theta2) + r3*cos(theta1 + theta2 + theta3)))/2 + (m3*r3*cos(theta1 + theta2 + theta3)*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2)))/2 - (m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) + L1*cos(theta1) - r4*cos(theta1 + theta2 + theta3 - theta4)))/2 + (m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) + L1*sin(theta1) - r4*sin(theta1 + theta2 + theta3 - theta4)))/2 + (m3*r3*sin(theta1 + theta2 + theta3)*(L2*cos(theta1 + theta2) + L1*cos(theta1) + r3*cos(theta1 + theta2 + theta3)))/2 - (m3*r3*cos(theta1 + theta2 + theta3)*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2) + L1*sin(theta1)))/2 + (m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) - r4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) - r4*sin(theta1 + theta2 + theta3 - theta4)))/2) + theta1_dot*theta3_dot*((m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 + (3*m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (3*m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 + (3*m3*r3*sin(theta1 + theta2 + theta3)*(L2*cos(theta1 + theta2) + r3*cos(theta1 + theta2 + theta3)))/2 - (3*m3*r3*cos(theta1 + theta2 + theta3)*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2)))/2 - (m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) + L1*cos(theta1) - r4*cos(theta1 + theta2 + theta3 - theta4)))/2 + (m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) + L1*sin(theta1) - r4*sin(theta1 + theta2 + theta3 - theta4)))/2 + (m3*r3*sin(theta1 + theta2 + theta3)*(L2*cos(theta1 + theta2) + L1*cos(theta1) + r3*cos(theta1 + theta2 + theta3)))/2 - (m3*r3*cos(theta1 + theta2 + theta3)*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2) + L1*sin(theta1)))/2 - (3*m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) - r4*cos(theta1 + theta2 + theta3 - theta4)))/2 + (3*m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) - r4*sin(theta1 + theta2 + theta3 - theta4)))/2) - 2*theta3_dot*theta4_dot*(m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)) - m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) - r4*cos(theta1 + theta2 + theta3 - theta4)) + m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) - r4*sin(theta1 + theta2 + theta3 - theta4))) - theta2_dot*theta4_dot*(2*m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)) - 2*m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)) - 2*m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) - r4*cos(theta1 + theta2 + theta3 - theta4)) + 2*m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) - r4*sin(theta1 + theta2 + theta3 - theta4))) + theta1_dot*theta4_dot*((m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 - (m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 + (m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 - (m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) + L1*cos(theta1) - r4*cos(theta1 + theta2 + theta3 - theta4)))/2 + (m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) + L1*sin(theta1) - r4*sin(theta1 + theta2 + theta3 - theta4)))/2 + (m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) - r4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) - r4*sin(theta1 + theta2 + theta3 - theta4)))/2) - theta1_dot*theta4_dot*((m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 + (3*m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (3*m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 - (m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) + L1*cos(theta1) - r4*cos(theta1 + theta2 + theta3 - theta4)))/2 + (m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) + L1*sin(theta1) - r4*sin(theta1 + theta2 + theta3 - theta4)))/2 - (3*m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) - r4*cos(theta1 + theta2 + theta3 - theta4)))/2 + (3*m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) - r4*sin(theta1 + theta2 + theta3 - theta4)))/2) + theta2_dot*theta3_dot*(2*m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)) - 2*m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)) + 2*m3*r3*sin(theta1 + theta2 + theta3)*(L2*cos(theta1 + theta2) + r3*cos(theta1 + theta2 + theta3)) - 2*m3*r3*cos(theta1 + theta2 + theta3)*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2)) - 2*m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) - r4*cos(theta1 + theta2 + theta3 - theta4)) + 2*m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) - r4*sin(theta1 + theta2 + theta3 - theta4)));
RHS(3,1) = u(3) - theta1_dot^2*(m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)) - m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) + L1*cos(theta1) - r4*cos(theta1 + theta2 + theta3 - theta4)) + m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) + L1*sin(theta1) - r4*sin(theta1 + theta2 + theta3 - theta4)) + m3*r3*sin(theta1 + theta2 + theta3)*(L2*cos(theta1 + theta2) + L1*cos(theta1) + r3*cos(theta1 + theta2 + theta3)) - m3*r3*cos(theta1 + theta2 + theta3)*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2) + L1*sin(theta1))) - (981*m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4)))/100 + theta5_dot^2*(m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))) - theta2_dot^2*(m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)) + m3*r3*sin(theta1 + theta2 + theta3)*(L2*cos(theta1 + theta2) + r3*cos(theta1 + theta2 + theta3)) - m3*r3*cos(theta1 + theta2 + theta3)*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2)) - m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) - r4*cos(theta1 + theta2 + theta3 - theta4)) + m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) - r4*sin(theta1 + theta2 + theta3 - theta4))) - (981*m4*r4*sin(theta1 + theta2 + theta3 - theta4))/100 - theta2_dot*theta5_dot*((m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 - (m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 + (m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2) + theta2_dot*theta5_dot*((m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 + (3*m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (3*m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2) - theta1_dot*theta5_dot*((m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 + (m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2) + theta1_dot*theta5_dot*((m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 + (3*m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 - (3*m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2) + (981*m3*r3*sin(theta1 + theta2 + theta3))/100 - theta1_dot*theta2_dot*((m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 + (m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 + (m3*r3*sin(theta1 + theta2 + theta3)*(L2*cos(theta1 + theta2) + r3*cos(theta1 + theta2 + theta3)))/2 - (m3*r3*cos(theta1 + theta2 + theta3)*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2)))/2 - (m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) + L1*cos(theta1) - r4*cos(theta1 + theta2 + theta3 - theta4)))/2 + (m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) + L1*sin(theta1) - r4*sin(theta1 + theta2 + theta3 - theta4)))/2 + (m3*r3*sin(theta1 + theta2 + theta3)*(L2*cos(theta1 + theta2) + L1*cos(theta1) + r3*cos(theta1 + theta2 + theta3)))/2 - (m3*r3*cos(theta1 + theta2 + theta3)*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2) + L1*sin(theta1)))/2 - (m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) - r4*cos(theta1 + theta2 + theta3 - theta4)))/2 + (m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) - r4*sin(theta1 + theta2 + theta3 - theta4)))/2) + theta1_dot*theta2_dot*((m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 - (3*m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 + (3*m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 - (3*m3*r3*sin(theta1 + theta2 + theta3)*(L2*cos(theta1 + theta2) + r3*cos(theta1 + theta2 + theta3)))/2 + (3*m3*r3*cos(theta1 + theta2 + theta3)*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2)))/2 - (m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) + L1*cos(theta1) - r4*cos(theta1 + theta2 + theta3 - theta4)))/2 + (m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) + L1*sin(theta1) - r4*sin(theta1 + theta2 + theta3 - theta4)))/2 + (m3*r3*sin(theta1 + theta2 + theta3)*(L2*cos(theta1 + theta2) + L1*cos(theta1) + r3*cos(theta1 + theta2 + theta3)))/2 - (m3*r3*cos(theta1 + theta2 + theta3)*(r3*sin(theta1 + theta2 + theta3) + L2*sin(theta1 + theta2) + L1*sin(theta1)))/2 + (3*m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) - r4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (3*m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) - r4*sin(theta1 + theta2 + theta3 - theta4)))/2) + theta3_dot*theta5_dot*(2*m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)) - 2*m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))) - theta4_dot*theta5_dot*(2*m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)) - 2*m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4)));
RHS(4,1) = u(4) + (981*m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4)))/100 - theta5_dot^2*(m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))) + theta2_dot^2*(m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)) - m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) - r4*cos(theta1 + theta2 + theta3 - theta4)) + m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) - r4*sin(theta1 + theta2 + theta3 - theta4))) + theta1_dot^2*(m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)) - m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) + L1*cos(theta1) - r4*cos(theta1 + theta2 + theta3 - theta4)) + m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) + L1*sin(theta1) - r4*sin(theta1 + theta2 + theta3 - theta4))) + (981*m4*r4*sin(theta1 + theta2 + theta3 - theta4))/100 + theta2_dot*theta5_dot*((m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 - (m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 + (m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2) - theta2_dot*theta5_dot*((m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 + (3*m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (3*m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2) + theta1_dot*theta5_dot*((m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 + (m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2) - theta1_dot*theta5_dot*((m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 + (3*m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 - (3*m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2) - theta3_dot*theta5_dot*(2*m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)) - 2*m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))) + theta4_dot*theta5_dot*(2*m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)) - 2*m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))) + theta1_dot*theta2_dot*((m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 + (m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 - (m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) + L1*cos(theta1) - r4*cos(theta1 + theta2 + theta3 - theta4)))/2 + (m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) + L1*sin(theta1) - r4*sin(theta1 + theta2 + theta3 - theta4)))/2 - (m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) - r4*cos(theta1 + theta2 + theta3 - theta4)))/2 + (m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) - r4*sin(theta1 + theta2 + theta3 - theta4)))/2) - theta1_dot*theta2_dot*((m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 - (3*m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 + (3*m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4))*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 - (m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) + L1*cos(theta1) - r4*cos(theta1 + theta2 + theta3 - theta4)))/2 + (m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) + L1*sin(theta1) - r4*sin(theta1 + theta2 + theta3 - theta4)))/2 + (3*m4*r4*sin(theta1 + theta2 + theta3 - theta4)*(L2*cos(theta1 + theta2) - r4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (3*m4*r4*cos(theta1 + theta2 + theta3 - theta4)*(L2*sin(theta1 + theta2) - r4*sin(theta1 + theta2 + theta3 - theta4)))/2);
RHS(5,1) = u(5) - theta2_dot^2*(m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4))) - theta1_dot^2*(m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4))) - theta3_dot^2*(m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))) - theta4_dot^2*(m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))) - theta1_dot*theta2_dot*((m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 + (m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2) - theta1_dot*theta2_dot*((3*m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (3*m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 - (m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 + (m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2) - theta2_dot*theta3_dot*((m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 + (m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2) + theta2_dot*theta4_dot*((m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 + (m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2) + theta2_dot*theta3_dot*((m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 - (3*m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 + (3*m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2) - theta2_dot*theta4_dot*((m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 - (3*m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 + (3*m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2) - theta1_dot*theta3_dot*((m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 + (m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 - (m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2) + theta1_dot*theta4_dot*((m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 + (m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 - (m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2) + theta1_dot*theta3_dot*((m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (3*m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 + (3*m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2) - theta1_dot*theta4_dot*((m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - L2*cos(theta1 + theta2) - L1*cos(theta1) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (3*m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)))/2 - (m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - L2*sin(theta1 + theta2) - L1*sin(theta1) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2 + (3*m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4)))/2) + 2*theta3_dot*theta4_dot*(m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + L4*cos(theta1 + theta2 + theta3 - theta4)) - m5*r5*cos(theta1 + theta2 + theta3 - theta4 + theta5)*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + L4*sin(theta1 + theta2 + theta3 - theta4))) - (981*m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5))/100;

dxdt=zeros(10,1);

% state variables derivative
dxdt(1) = x(6);
dxdt(2) = x(7);
dxdt(3) = x(8);
dxdt(4) = x(9);
dxdt(5) = x(10);
dxdt(6:10) = M\RHS;

end